#include <iostream>
#include "../include/calib_odom/Odom_Calib.hpp"
using namespace std;


//设置数据长度,即多少数据计算一次
void OdomCalib::Set_data_len(int len)
{
    data_len = len;
    A.conservativeResize(len*3,9);
    b.conservativeResize(len*3);
    A.setZero();
    b.setZero();
}


/*
输入:里程计和激光数据

TODO:
构建最小二乘需要的超定方程组
Ax = b

*/

Eigen::Matrix<double,1,9> calc_row1(Eigen::Vector3d & Odom) {
    Eigen::Matrix<double,1,9> r= Eigen::Matrix<double,1,9>::Zero(1, 9);
    //for(int i = 0; i < 9; ++i) {
        //r << 0;
    //}
    r[0] = Odom[0];
    r[1] = Odom[1];
    r[2] = Odom[2];
    return r;
}

Eigen::Matrix<double,1,9> calc_row2(Eigen::Vector3d & Odom) {
    Eigen::Matrix<double,1,9> r=Eigen::Matrix<double,1,9>::Zero(1, 9);
    //Eigen::Matrix<double,1,9> r;
    //for(int i = 0; i < 9; ++i) {
        //r << 0;
    //}
    r[3] = Odom[0];
    r[4] = Odom[1];
    r[5] = Odom[2];
    return r;
}
Eigen::Matrix<double,1,9> calc_row3(Eigen::Vector3d & Odom) {
    Eigen::Matrix<double,1,9> r=Eigen::Matrix<double,1,9>::Zero(1, 9);
    //Eigen::Matrix<double,1,9> r;
    //for(int i = 0; i < 9; ++i) {
        //r << 0;
    //}
    r[6] = Odom[0];
    r[7] = Odom[1];
    r[8] = Odom[2];
    return r;
}

bool OdomCalib::Add_Data(Eigen::Vector3d Odom,Eigen::Vector3d scan)
{

    cout << "A shape=" << A.rows() << "," << A.cols() << endl;
    cout << "b shape=" << b.rows() << "," << b.cols() << endl;
    if(now_len * 3 + 2 >= A.rows()) {
        /* space of A is not enought */
        A.conservativeResize(A.rows()+3*10, 9);
        b.conservativeResize(b.rows()+3*10, 1);
    }
    //TODO: 构建超定方程组
    //A.conservativeResize(A.rows()+3, 9);
    //cout << "A.row shape=" << A.row(now_len*3).rows() << "," << A.row(now_len*3).cols() << endl;
    A.row(now_len*3) = calc_row1(Odom);
    A.row(now_len*3+1) = calc_row2(Odom);
    A.row(now_len*3+2) = calc_row3(Odom);
    b(now_len*3) = scan[0];
    b(now_len*3+1) = scan[1];
    b(now_len*3+2) = scan[2];
    //end of TODO
    now_len++;
    return true;
}

/*
 * TODO:
 * 求解线性最小二乘Ax=b
 * 返回得到的矫正矩阵
*/
Eigen::Matrix3d OdomCalib::Solve()
{
    Eigen::Matrix3d correct_matrix;

    //TODO:求解线性最小二乘
    Eigen::MatrixXd x = (A.transpose()*A).inverse()*A.transpose()*b;
    for(int i = 0; i < 9; ++i) {
        int r = i/3;
        int c = i%3;
        correct_matrix(r, c) = x(i, 0);
    }
    Eigen::MatrixXd err = A*x-b;
    double sum = 0;
    for(int i = 0; i < err.rows(); ++i) {
        sum += err(i,0);
    }

    //std::cout << "error:\n" << A*x-b << std::endl;
    cout << "sum error=" << sum << endl;

    //end of TODO

    return correct_matrix;
}

/* 用于判断数据是否满
 * 数据满即可以进行最小二乘计算
*/
bool OdomCalib::is_full()
{
    if(now_len%data_len==0&&now_len>=1)
    {
        now_len = data_len;
        return true;
    }
    else
        return false;
}

/*
 * 数据清零
*/
void OdomCalib::set_data_zero()
{
    A.setZero();
    b.setZero();
}
